﻿using System.ServiceModel;

using Coding4Fun.Cannon.Robot;
using Coding4Fun.Cannon.Robot.Types;

namespace Coding4Fun.Cannon.Wcf
{
	// NOTE: You can use the "Rename" command on the "Refactor" menu to change the class name "DriveService" in both code and config file together.
	[ServiceBehavior(InstanceContextMode = InstanceContextMode.Single)]
	public class DriveService : IDriveService
	{
		private static AmpFlow MotorController
		{
			get
			{
				if (_motorController == null)
				{
					var settings = (new SettingsService());
					_motorController = new AmpFlow(settings.GetDriveComport());

					_leftMaxPower = settings.GetDriveMaxPowerLeftMotor();
					_rightMaxPower = settings.GetDriveMaxPowerRightMotor();

					_motorController.OpenComPort();
				}

				return _motorController;
			}
		}
		private static AmpFlow _motorController;
		private static int _leftMaxPower;
		private static int _rightMaxPower;
		private static int _leftCurrentPower;
		private static int _rightCurrentPower;

		#region Implementation of IDriveService
		
		public void Stop()
		{
			SetPowerByMotor(0, 0);
		}

		public void SetPower(int power)
		{
			SetPowerByMotor(power, power);
		}

		public void SetPowerByMotor(int leftPower, int rightPower)
		{
			_leftCurrentPower = TwoHundredToTwoHundredFiftySix(ValidatePowerLevel(leftPower));
			_rightCurrentPower = TwoHundredToTwoHundredFiftySix(ValidatePowerLevel(rightPower));

			MotorController.SetMotorSpeed(RobotTypes.MotorChannel.A, _leftCurrentPower);
			MotorController.SetMotorSpeed(RobotTypes.MotorChannel.B, _rightCurrentPower);
		}

		internal void SetDriveMaxPower(int power)
		{
			SetDriveMaxPowerByMotor(power, power);
		}

		internal void SetDriveMaxPowerByMotor(int leftPower, int rightPower)
		{
			_leftMaxPower = ValidatePowerLevel(leftPower);
			_rightMaxPower = ValidatePowerLevel(rightPower);

			var resetMotors = false;
			if (_leftCurrentPower > _leftMaxPower)
			{
				resetMotors = true;
				_leftCurrentPower = _leftMaxPower;
			}
			else if (_rightCurrentPower > _rightMaxPower)
			{
				resetMotors = true;
				_rightCurrentPower = _rightMaxPower;
			}

			if (resetMotors)
				SetPowerByMotor(_leftCurrentPower, _rightCurrentPower);
		}

		private static int ValidatePowerLevel(int value)
		{
			if (value > 100)
				value = 100;
			else if (value < -100)
				value = -100;

			return value;
		}

		private static int TwoHundredToTwoHundredFiftySix(int value)
		{
			return (int) (-127*(value/100.0));
		}

		#endregion
	}
}
